﻿/*      硬件抽象层
 * 读取硬件状态（寻磁，伺服电机，IO模块）
 * 底层数据粗加工，生成AGV状态（急停，按键，空料，满料）
 * 提供硬件操作，查询接口
 */
#include "agvhardware.h"
Agvhardware::Agvhardware( int argc, char** argv, Maindeal* maindeal, QObject* parent ) : QObject( parent ), maindeal( maindeal ), argc( argc ), argv( argv ) {}

Agvhardware::~Agvhardware() {
    thread_1->quit();  //退出线程1
    thread_2->quit();  //退出线程2
}

//  Agvhardware 物料光感数目和状态判断
void Agvhardware::ScanSignal() {}

void Agvhardware::resetServoDriverAlarm() {
    //  io_module->setDo(IO::LEFT_SERVO_DRIVER_ENABLE, IO::RESET);
    //  io_module->setDo(IO::RIGHT_SERVO_DRIVER_ENABLE, IO::RESET);
    //  QTimer::singleShot(1000, this, [=]() {
    //    io_module->setDo(IO::LEFT_SERVO_DRIVER_ENABLE, IO::SET);
    //    io_module->setDo(IO::RIGHT_SERVO_DRIVER_ENABLE, IO::SET);
    //  });
}

/**
 * @brief Agvhardware::getGoodsState 获取货物状态
 * @return
 */
Agvhardware::GoodsState Agvhardware::getGoodsState() {
    return goods_state;
}

void Agvhardware::slt50msTimer() {
    /************************************启动按钮*************************************/
    buttonEvent();
    /************************************光感扫描*************************************/
    ScanSignal();
    /************************************传输完成检查，前提是自动开始才可以执行，并且完成后会切换成“对接完成状态”**/
    // transmitAutoDriverDisable();
    /************************************自动模式的led灯显示*************************************/
    autoModeLed( maindeal->navigator()->isAutoModeEnable() );
    /************************************货叉控制*************************************/
    ForkliftHandle();
}

void Agvhardware::slt500msTimer() {
    /************************************灯光控制*************************************/
    lightControl();
    /************************************音乐控制*************************************/
    musicControl();
}

void Agvhardware::slt100msTimer() {
    /*************************************严重的异常状态*************************************/
    if ( isNeedAlarm() ) {  //紧急停止事件发生，关闭所有AGV控制的功能，行走由navi获取当前急停状态来控制驱动停止
        setAlarm( true );
        if ( getAgvState() != AGV_EMERGENCY ) {  //内部做只触发一次的函数，并且非急停按钮触发的异常在没清除情况下，驱动电机是不会被失能处理!
            setAgvState( AGV_EMERGENCY );        //切换急停状态
            setForkliftState( STOP );
            stopCharge();
        }
        return;
    }
    /*************************************壁障状态，要放到所有检测的最后*************************************/
    if ( maindeal->navigator()->isObstacleFunctionEnable() && maindeal->navigator()->isAutoModeEnable() ) {
        if ( isObstacleStop( maindeal->navigator()->isForward() ) || isSlamObstacleStop( 100 ) ) {
            if ( getAgvState() == AGV_RUNNING ) {  //让其发送一次数据的作用
                last_agv_state = AGV_RUNNING;      //记录上一次的状态
                setAgvState( AGV_OBSTACLE );       // 切换避障状态
            }
            return;  //不能移动，会影响下面恢复
        }
    }
    /*************************************复位状态*************************************/
    setAlarm( false );
    if ( getAgvState() == AGV_EMERGENCY ) {
        setAgvState( AGV_STOP );  // 切换停止状态
        emit sigSendTips( "已清除急停状态" );
    }
    else if ( getAgvState() == AGV_OBSTACLE ) {  // 切换避障状态
        setAgvState( last_agv_state );           // 避障停止后恢复上一次的状态
        emit sigSendTips( "已清除避障状态" );
    }
}

void Agvhardware::stopCharge() {
    chargingFinish = false;
    setChargeEnable( false );  //关闭充电开关
#ifdef PILE
    unsigned short port = 50000;
#endif
#ifdef ZHONGSHENG
    unsigned short port = 8899;
#endif
    // 如果在充电桩站点，检查充电桩收起
    if ( maindeal->navigator()->slamRoutePlanning()->isChargeNum( maindeal->navigator()->getSlamId() ) ) {
        QTimer::singleShot( 100, this, [&]() {
            QString ip = maindeal->navigator()->slamRoutePlanning()->getChargeIp( maindeal->navigator()->getSlamId() );
            if ( ip.trimmed().size() == 0 ) { emit sigSendTips( "警告，尝试收起充电桩，但获取充电桩IP为空" ); }
            else if ( maindeal->agvhardware()->chargingPileModule()->chargingClose() ) {
                maindeal->agvhardware()->chargingPileModule()->tcpOpen( ip, port );
            }
            QTimer::singleShot( 5000, this, [&]() {
                if ( getAgvState() == AGV_CHARGING || getAgvState() == AGV_PRECHARGE ) { setAgvState( AGV_ARRIVE ); }
                emit sigSendTips( "尝试收起充电桩，请注意安全" );
                if ( !chargingFinish ) { emit sigSendTips( "充电桩通讯异常！" ); }

            } );
        } );
    }
}

void Agvhardware::startCharge() {

    chargingFinish = false;
#ifdef PILE
    unsigned short port = 50000;
#endif
#ifdef ZHONGSHENG
    unsigned short port = 8899;
#endif

    if ( !maindeal->navigator()->slamRoutePlanning()->isChargeNum( maindeal->navigator()->getSlamId() ) ) {
        emit sigSendTips( "警告，当前不是充电id" );
        return;
    }

    if ( maindeal->agvhardware()->getAgvState() != Agvhardware::AGV_ARRIVE ) {
        emit sigSendTips( "警告，小车非自动模式-到达状态，无法强制充电" );
        return;
    }

    emit sigSendTips( "开始执行自动充电操作" );

    QString ip = maindeal->navigator()->slamRoutePlanning()->getChargeIp( maindeal->navigator()->getSlamId() );

    if ( ip.trimmed().size() == 0 ) {
        emit sigSendTips( "警告，获取充电桩IP为空，无法强制充电" );
        return;
    }
    emit sigSendTips( QString( "充电桩IP: %1" ).arg( ip ) );

    if ( maindeal->agvhardware()->chargingPileModule()->chargingOpen() ) {
        qDebug() << "重新连接充电桩";
        maindeal->agvhardware()->chargingPileModule()->tcpOpen( ip, port );
    }
    QTimer::singleShot( 5000, this, [&]() {
        if ( chargingFinish ) {
            maindeal->agvhardware()->setAgvState( Agvhardware::AGV_PRECHARGE );
            maindeal->agvhardware()->setChargeEnable( true );
        }
        else {
            sigSendTips( "警告，获取充电桩通讯异常，无法强制充电" );
        }
    } );
}

void Agvhardware::slt1000msTimer() {
    // static int time_2 = 0, time_3 = 0, time_4 = 0;
    static int time_4 = 0;

    /*************************************电池低压检测*************************************/
    Battery_Ropeencoder_can::BatteryData_YJT battery_data = Battery_Ropeencoder_can_Module->getBatteryData();
    if ( battery_data.soc < Param::lower_power ) {  //设置低压百分比
        if ( time_4++ > 30 ) {
            time_4       = 0;
            is_low_power = true;
            emit sigSendTips( QString( "警告，电量过低，剩余电量：%1%" ).arg( battery_data.soc ) );
        }
    }
    else {
        time_4       = 0;
        is_low_power = false;
    }

    /*************************************电池充电检测*************************************/
    if ( battery_data.charge_relay_state && getAgvState() == AGV_CHARGING && getAgvState() == AGV_PRECHARGE ) {
        setAgvState( AGV_CHARGING );
        emit sigSendTips( QString( "开始充电，当前电量：%1%" ).arg( battery_data.soc ) );
    }
    else if ( battery_data.charge_relay_state && getAgvState() == AGV_CHARGING && battery_data.charge_state == Battery_Ropeencoder_can::Charged ) {
        emit sigSendTips( QString( "充电完成，当前电量：%1%" ).arg( battery_data.soc ) );
        stopCharge();
    }
}

void Agvhardware::sltLoseConnect( const QString& message ) {  //  Agvhardware 硬件模块通讯丢失
    emit sigSendTips( "错误，" + message + "断开连接" );
    maindeal->navigator()->setEmergencyStop( true );
}

void Agvhardware::sltRFIDRecvieNaviParam( const int& rfidId ) {
    Q_UNUSED( rfidId )
}

/**
 * @brief Agv::init 初始化 构建navigator对象和agvbse的初始化。
 * 之所以没有在构造函数创建一下对象，是为了将agv和navi放在同一个线程里面
 */
void Agvhardware::sltAgvHardwareInit() {  //  Agvhardware 初始化数据
    // qDebug() << "agvhardware_currentThreadId()==" << QThread::currentThreadId();
    /***********************创建线程对象*******************************************************/
    //线程1创建
    thread_1 = new QThread;
// can_module = new CAN();

#ifdef ARESMOTECFORKDRIVE
    forklift_drive_module = new AresMotecDrive();
#endif

#ifdef DMKEMOTECFORKDRIVE
    forklift_drive_module = new DmakeMotecDrive();
#endif

#ifdef BUKEKEDISHIFORKDRIVE
    forklift_drive_module = new BukeKedishiDrive();
#endif

    qDebug() << "forklift_drive can dw_DeviceHandle" << forklift_drive_module->dw_DeviceHandle;
    Battery_Ropeencoder_can_Module = new Battery_Ropeencoder_can( forklift_drive_module->dw_DeviceHandle );
    forklift_drive_module->moveToThread( thread_1 );
    Battery_Ropeencoder_can_Module->moveToThread( thread_1 );

    connect( thread_1, SIGNAL( started() ), forklift_drive_module, SLOT( sltForkLiftDrive_Init() ) );
    connect( thread_1, SIGNAL( started() ), Battery_Ropeencoder_can_Module, SLOT( sltCanInit_1() ) );

    connect( thread_1, SIGNAL( finished() ), thread_1, SLOT( deleteLater() ) );
    thread_1->start();
    thread_1->setPriority( QThread::TimeCriticalPriority );

    //线程2创建
    thread_2  = new QThread;
    io_module = new IO();
    io_module->moveToThread( thread_2 );

    connect( thread_2, SIGNAL( started() ), io_module, SLOT( sltIOInit() ) );
    connect( thread_2, SIGNAL( finished() ), thread_2, SLOT( deleteLater() ) );
    thread_2->start();
    thread_2->setPriority( QThread::TimeCriticalPriority );

#ifdef PILE
    charging_module = new Charging_Pile_IO();
#endif

#ifdef ZHONGSHENG
    charging_module = new chargingZhongSheng_IO();
#endif

    //重要信号槽联系
    connect( forklift_drive_module, SIGNAL( sigSendTips( QString ) ), this, SIGNAL( sigSendTips( QString ) ) );
    // UPDATE 2022.09.17 修改CAN类源码，增加CAN的伺服通讯断连接报警和节点超时保护
    //  connect( can_module, SIGNAL( sigLoseConnect( QString ) ), this, SLOT( sltLoseConnect( QString ) ) );
    connect( io_module, SIGNAL( sigSendTips( QString ) ), this, SIGNAL( sigSendTips( QString ) ) );
    connect( io_module, SIGNAL( sigLoseConnect( QString ) ), this, SLOT( sltLoseConnect( QString ) ) );

    connect( charging_module, SIGNAL( sigSendTips( QString ) ), this, SIGNAL( sigSendTips( QString ) ) );
    connect( charging_module, SIGNAL( sigFinish() ), this, SLOT( sltchargingFinish() ) );
    //  connect(battery_module, SIGNAL(sigSendTips(QString)), this, SIGNAL(sigSendTips(QString)));
    connect( Battery_Ropeencoder_can_Module, SIGNAL( sigSendTips( QString ) ), this, SIGNAL( sigSendTips( QString ) ) );

    /***********************设置定时器********************************************************/
    auto timer1000ms = new QTimer();
    connect( timer1000ms, SIGNAL( timeout() ), this, SLOT( slt1000msTimer() ) );
    timer1000ms->start( 1000 );

    QTimer* timer500ms = new QTimer();
    connect( timer500ms, SIGNAL( timeout() ), this, SLOT( slt500msTimer() ) );
    timer500ms->start( 500 );

    QTimer* timer100ms = new QTimer();
    connect( timer100ms, SIGNAL( timeout() ), this, SLOT( slt100msTimer() ) );
    timer100ms->start( 100 );

    QTimer* timer50ms = new QTimer();
    connect( timer50ms, SIGNAL( timeout() ), this, SLOT( slt50msTimer() ) );
    timer50ms->start( 50 );

    //复位硬件状态
    resetHardwareState();
}

/**
 * @brief Agvhardware::isAllowTransmit 是否禁止传输
 */
bool Agvhardware::isAllowForklift() {
    // TEST 修改传输类型4：任务传输限制

    switch ( getAgvState() ) {
    case AGV_ARRIVE:
    case AGV_LIFT_STOP:
        return true;
    default:
        return false;
    }
}

// UPDATE 2022.11.05 isOnlyFrontObsStop的左右避障开关控制
bool Agvhardware::isOnlyLeftObsStop() {
    if ( io_module->getDi( IO::LEFT_OBSTACLE_STOP_LAYER ) == IO::RESET                                           //
         && io_module->getDi( IO::RIGHT_OBSTACLE_STOP_LAYER ) == IO::SET                                         //
         && ( io_module->getDi( IO::LEFT_OBSTACLE_STOP_LAYER ) == IO::SET || ( Param::close_obs_left == 0 ) )    //
         && ( io_module->getDi( IO::RIGHT_OBSTACLE_STOP_LAYER ) == IO::SET || ( Param::close_obs_right == 0 ) )  //
         ) {
        return true;
    }
    return false;
}
bool Agvhardware::isOnlyRightObsStop() {
    if ( io_module->getDi( IO::RIGHT_OBSTACLE_STOP_LAYER ) == IO::RESET                                          //
         && io_module->getDi( IO::LEFT_OBSTACLE_STOP_LAYER ) == IO::SET                                          //
         && ( io_module->getDi( IO::LEFT_OBSTACLE_STOP_LAYER ) == IO::SET || ( Param::close_obs_left == 0 ) )    //
         && ( io_module->getDi( IO::RIGHT_OBSTACLE_STOP_LAYER ) == IO::SET || ( Param::close_obs_right == 0 ) )  //
         ) {
        return true;
    }
    return false;
}

/**
 * @brief Agv::setAutoCharge 设置自动充电状态
 * @param state ture打开 false关闭
 */
void Agvhardware::setChargeEnable( bool state ) {
    /******************打开充电继电器******************/
    if ( state ) {
        // 先断开一下
        Battery_Ropeencoder_canModule()->charge( false );
        ioModule()->setDo( IO::CHARGE, IO::RESET );

        QTimer::singleShot( 2000, this, [=]() {
            if ( getAgvState() == AGV_PRECHARGE ) {
                ioModule()->setDo( IO::CHARGE, IO::SET );
                Battery_Ropeencoder_canModule()->charge( true );  // 等待充电铁块接触后再打开继电器
            }
            else {
                emit sigSendTips( "错误：小车状态不正确，无法打开充电继电器" );
            }
        } );
    }
    /******************关闭充电继电器******************/
    else {
        Battery_Ropeencoder_canModule()->charge( false );
        ioModule()->setDo( IO::CHARGE, IO::RESET );
        // if (agv_state == AGV_PRECHARGE || agv_state == AGV_CHARGING) setAgvState(AGV_ARRIVE);
    }
}

/**
 * @brief Agv::setAgvState 设置AGV的状态
 * @param agvstate 是AgvState结构体的成员
 */
void Agvhardware::setAgvState( const Agvhardware::AgvState& agvstate ) {
    agv_state = agvstate;
}

void Agvhardware::waitingCloseAgv() {
    io_module->resetAllDo();
    qDebug() << "复位全部输出io";
    forklift_drive_module->CanStopAndClose();
    qDebug() << "正常退出软件成功";
}

/**
 * @brief Agv::isNeedAlarm 判断异常情况是否发生
 * @return true有异常，false没异常
 */
bool Agvhardware::isNeedAlarm() {
    if ( isNeedEmergencyStop() ) { return true; }                     //需要触发急停：急停按钮按下
    if ( isHardwareError() ) { return true; }                         //硬件错误：驱动器错误
    if ( maindeal->navigator()->isEmergencyStop() ) { return true; }  //导航急停：根据需求触发，需要清除异常才可以复位(任务超时，导航错误，寻磁脱轨)
    return false;
}

// void Agvhardware::setCannotTransmit( bool sw ) {
//    is_cannot_transmit = sw;
//}

/**
 * @brief Agv::lightControl 灯光控制
 * @param isForward
 */
void Agvhardware::lightControl() {    // UPDATE 2022.08.18 灯光判断重改
    if ( isAlarm() || is_low_power )  //异常：红闪
    {
        io_module->setDo( IO::GREEN_LED, IO::RESET );
        io_module->setDo( IO::YELLOW_LED, IO::RESET );
        io_module->setDo( IO::RED_LED, IO::TOGGLE );
        return;
    }

    if ( !maindeal->navigator()->isAutoModeEnable() ) {  //手动模式：

        io_module->setDo( IO::GREEN_LED, IO::RESET );
        io_module->setDo( IO::YELLOW_LED, IO::TOGGLE );
        io_module->setDo( IO::RED_LED, IO::RESET );
        return;
    }

    switch ( getAgvState() ) {
    case AGV_RUNNING:  //自动模式：行走中  绿闪
        io_module->setDo( IO::GREEN_LED, IO::TOGGLE );
        io_module->setDo( IO::YELLOW_LED, IO::RESET );
        io_module->setDo( IO::RED_LED, IO::RESET );
        break;
    case AGV_OBSTACLE:  //自动模式：遇到障碍物，红常亮
        io_module->setDo( IO::GREEN_LED, IO::RESET );
        io_module->setDo( IO::YELLOW_LED, IO::RESET );
        io_module->setDo( IO::RED_LED, IO::SET );
        break;
    case AGV_TRAFFIC:  //自动模式：交管， 黄常亮
        io_module->setDo( IO::GREEN_LED, IO::RESET );
        io_module->setDo( IO::YELLOW_LED, IO::SET );
        io_module->setDo( IO::RED_LED, IO::RESET );
        break;
    default:  // 其他状态 绿常亮
        io_module->setDo( IO::GREEN_LED, IO::SET );
        io_module->setDo( IO::YELLOW_LED, IO::RESET );
        io_module->setDo( IO::RED_LED, IO::RESET );
    }
}

void Agvhardware::musicControl() {
    static int emergency_time = 0, obs_time = 0;
    if ( getAgvState() == AGV_EMERGENCY ) {
        if ( ++emergency_time > 4 ) {
            io_module->setDo( IO::WORKING_MUSIC_1, IO::RESET );  //避障播报
            io_module->setDo( IO::WORKING_MUSIC_2, IO::RESET );  //传输异常播报
            io_module->setDo( IO::WORKING_MUSIC_3, IO::RESET );  //电池低压
            io_module->setDo( IO::WORKING_MUSIC_4, IO::SET );    //紧急报警
            io_module->setDo( IO::WORKING_MUSIC_5, IO::RESET );  //行驶音乐
        }
        else {
            io_module->setDo( IO::WORKING_MUSIC_1, IO::RESET );
            io_module->setDo( IO::WORKING_MUSIC_2, IO::RESET );
            io_module->setDo( IO::WORKING_MUSIC_3, IO::RESET );
            io_module->setDo( IO::WORKING_MUSIC_4, IO::RESET );
            io_module->setDo( IO::WORKING_MUSIC_5, IO::RESET );
        }
        return;
    }
    else if ( is_low_power ) {
        io_module->setDo( IO::WORKING_MUSIC_1, IO::RESET );
        io_module->setDo( IO::WORKING_MUSIC_2, IO::RESET );
        io_module->setDo( IO::WORKING_MUSIC_3, IO::SET );
        io_module->setDo( IO::WORKING_MUSIC_4, IO::RESET );
        io_module->setDo( IO::WORKING_MUSIC_5, IO::RESET );
    }
    else if ( getAgvState() == AGV_OBSTACLE ) {
        if ( ++obs_time > 4 ) {
            io_module->setDo( IO::WORKING_MUSIC_1, IO::SET );    //避障播报
            io_module->setDo( IO::WORKING_MUSIC_2, IO::RESET );  //传输异常播报
            io_module->setDo( IO::WORKING_MUSIC_3, IO::RESET );  //电池低压
            io_module->setDo( IO::WORKING_MUSIC_4, IO::RESET );  //紧急报警
            io_module->setDo( IO::WORKING_MUSIC_5, IO::RESET );  //行驶音乐
        }
        else {
            io_module->setDo( IO::WORKING_MUSIC_1, IO::RESET );
            io_module->setDo( IO::WORKING_MUSIC_2, IO::RESET );
            io_module->setDo( IO::WORKING_MUSIC_3, IO::RESET );
            io_module->setDo( IO::WORKING_MUSIC_4, IO::RESET );
            io_module->setDo( IO::WORKING_MUSIC_5, IO::RESET );
        }
        return;
    }
    else if ( getAgvState() == AGV_RUNNING ) {
        io_module->setDo( IO::WORKING_MUSIC_1, IO::RESET );  //避障播报
        io_module->setDo( IO::WORKING_MUSIC_2, IO::RESET );  //传输异常播报
        io_module->setDo( IO::WORKING_MUSIC_3, IO::RESET );  //电池低压
        io_module->setDo( IO::WORKING_MUSIC_4, IO::RESET );  //紧急报警
        io_module->setDo( IO::WORKING_MUSIC_5, IO::SET );    //行驶音乐
    }
    else {
        io_module->setDo( IO::WORKING_MUSIC_1, IO::RESET );
        io_module->setDo( IO::WORKING_MUSIC_2, IO::RESET );
        io_module->setDo( IO::WORKING_MUSIC_3, IO::RESET );
        io_module->setDo( IO::WORKING_MUSIC_4, IO::RESET );
        io_module->setDo( IO::WORKING_MUSIC_5, IO::RESET );
    }
    //复位
    emergency_time = 0;
    obs_time       = 0;
}

/**
 * @brief Agv::abortHandler 崩溃事件处理
 */
void Agvhardware::abortHandler() {
    qCritical() << "异常，程序崩溃!";
    io_module->breakHandler();
    qCritical() << "崩溃IO处理";
    forklift_drive_module->CanStopAndClose();
    qCritical() << "异常关闭软件完成";
}

/**
 * @brief Agv::buttonEvent 按钮触发事件
 */
void Agvhardware::buttonEvent() {
    //启动按钮
    static bool is_press_button = false;
    if ( io_module->getDi( IO::ACTIVATE ) == IO::SET ) { is_press_button = true; }
    if ( is_press_button && ( io_module->getDi( IO::ACTIVATE ) == IO::RESET ) ) {
        is_press_button = false;
        emit sigStartButtonHit();
    }

    //复位按钮
    static bool is_press_reset_button = false;
    if ( io_module->getDi( IO::RESET_BUTTON ) == IO::SET /*|| io_module->getDi(IO::BUMPER_STRIP) == IO::RESET*/ ) { is_press_reset_button = true; }
    if ( is_press_reset_button && ( io_module->getDi( IO::RESET_BUTTON ) == IO::RESET ) ) {

        maindeal->navigator()->clearEmergencyState();
        emit sigSendTips( tr( "清除异常状态成功!" ) );
        maindeal->navigator()->resetAllSwitchAndParam();
        maindeal->agvhardware()->resetHardwareState();
        maindeal->agvhardware()->stopCharge();
        maindeal->setCancelMission( false );  //复位任务清除状态
        emit sigSendTips( "关闭且复位所有模式和状态成功!" );
        is_press_reset_button = false;
    }

    //手自动切换按钮
    static bool is_auto_button = false;
    if ( is_auto_button && io_module->getDi( IO::AUTO_MODE ) == IO::SET ) {
        is_auto_button = false;
        onAutoChange( is_auto_button );
    }
    else if ( !is_auto_button && ( io_module->getDi( IO::AUTO_MODE ) == IO::RESET ) ) {
        is_auto_button = true;
        onAutoChange( is_auto_button );
    }

    //叉车地盘控制（行走，货叉，旋转）自动手动切换2023.07.24
    static bool is_fork_auto_button = false;
    if ( io_module->getDi( IO::FORK_AUTO_MODE ) == IO::SET && io_module->getDi( IO::FORK_HANDLE_MODE ) == IO::RESET ) {
        //自动模式
        is_fork_auto_button = true;
    }
    if ( io_module->getDi( IO::FORK_AUTO_MODE ) == IO::RESET && io_module->getDi( IO::FORK_HANDLE_MODE ) == IO::SET ) {
        //手动模式
        is_fork_auto_button = false;
    }
    onForkAutoChange( is_fork_auto_button );
    //叉车地盘控制（行走，货叉，旋转）自动手动切换2023.07.24
}

void Agvhardware::onForkAutoChange( bool isFrokAuto ) {
    if ( isFrokAuto ) {
        maindeal->navigator()->setForkVeersettingrad( 0 );
        maindeal->navigator()->setForkWalksettingspeed( 0 );
        setForkliftState( Agvhardware::STOP );
        canModule()->switchAutoModel();
    }
    else {
        canModule()->switchHandleModel();
    }
}

void Agvhardware::onAutoChange( bool isAuto ) {
    if ( !isAuto ) {
        maindeal->navigator()->setForkVeersettingrad( 0 );
        maindeal->navigator()->setForkWalksettingspeed( 0 );
        setForkliftState( Agvhardware::STOP );
    }
}

void Agvhardware::sltchargingFinish() {
    chargingFinish = true;
}

/**
 * @brief Agv::autoModeLed 自动模式指示灯
 */
void Agvhardware::autoModeLed( bool sw ) {
    if ( sw ) {
        io_module->setDo( IO::ACTIVATE_LED, IO::SET );
        io_module->setDo( IO::OUTLINE_LAMP, IO::SET );
    }
    else {
        io_module->setDo( IO::ACTIVATE_LED, IO::RESET );
        io_module->setDo( IO::OUTLINE_LAMP, IO::RESET );
    }
}

/**
 * @brief Agv::isLowPower 判断电池是否低压
 * @return is_low_power  true：是   fales：否
 */
bool Agvhardware::isLowPower() const {
    return is_low_power;
}

/**
 * @brief Agvhardware::isAgvArriveSignal 是否到位机台，判断AGV最前端光感状态
 */
bool Agvhardware::isAgvArriveSignal() {
    return false;
    //  // UPDATE 2022.11.05 到位光感开关
    //  if (Param::arrive_signal_open_close == 0) { return true; }
    //  if (io_module->getDi(IO::ARRIVE_SIGNAL) == IO::SET) {
    //    return true;
    //  } else {
    //    return false;
    //  }
}

/**
 * @brief Agv::isObstacleSlowDown 判断是否需要避障减速
 * @param isForward 前进的方向.根据前进的方向确定前后避障.
 * @return true:需要减速 false:不需要
 */
bool Agvhardware::isObstacleSlowDown( bool isForward ) {
    if ( isForward ) {
        if ( io_module->getDi( IO::LEFT_OBSTACLE_SLOW_LAYER ) == IO::RESET ) { return true; }
    }
    else {
        if ( io_module->getDi( IO::RIGHT_OBSTACLE_SLOW_LAYER ) == IO::RESET ) { return true; }
    }
    return false;
}
/**
 * @brief Agv::isObstacleStop 判断是否需要避障停车
 * @param isForward 前进的方向.根据前进的方向确定前后避障.
 * @return true:需要急停 false:不需要
 */
bool Agvhardware::isObstacleStop( bool isForward ) {
    Q_UNUSED( isForward )

    /******************************************侧边避障判断*******************************************************************/
    //侧边壁障容易误触发，兴颂的120度模块
    bool       is_alm     = false;
    int        time_limit = 2;  //延时200ms
    static int time       = 0;
    QString    tips;
    bool       is_open = true;
    // UPDATE 2022.11.01 isObstacleStop加入左右避障控制开关
    if ( io_module->getDi( IO::LEFT_OBSTACLE_STOP_LAYER ) == IO::RESET && is_open && left_obs_flag ) {
        if ( getAgvState() == AGV_RUNNING ) { tips = "警告，左避障急停层触发!"; }
        is_alm = true;
    }
    else if ( io_module->getDi( IO::RIGHT_OBSTACLE_STOP_LAYER ) == IO::RESET && is_open && right_obs_flag ) {
        if ( getAgvState() == AGV_RUNNING ) { tips = "警告，右避障急停层触发!"; }
        is_alm = true;
    }
    else if ( !maindeal->navigator()->isForward() && io_module->getDi( IO::LEFT_FORK_DETECTION ) == IO::SET && is_open ) {
        if ( getAgvState() == AGV_RUNNING ) { tips = "警告，左货叉头急停触发!"; }
        is_alm = true;
    }
    else if ( !maindeal->navigator()->isForward() && io_module->getDi( IO::RIGHT_FORK_DETECTION ) == IO::SET && is_open ) {
        if ( getAgvState() == AGV_RUNNING ) { tips = "警告，右货叉头急停触发!"; }
        is_alm = true;
    }
    //    else if ( !isForward && io_module->getDi( IO::BACK_OBSTACLE_STOP_LAYER ) == IO::RESET && is_open && getForkPosState() == Agvhardware::FORK_L_POS ) {
    //        if ( getAgvState() == AGV_RUNNING ) { tips = "警告，后避障急停层触发!"; }
    //        is_alm = true;
    //    }

    //信号消抖处理，避免误判
    if ( is_alm ) {
        if ( time++ >= time_limit ) {
            emit sigSendTips( tips );
            return true;
        }
    }
    else {
        time = 0;
    }
    return false;
}

bool Agvhardware::isSlamObstacleStop( int T ) {
    int        time_delay  = Param::slam_obs_timer / T;  // UPDATE 2022.06.23 绕障前延时
    static int time_count  = time_delay;
    int        time_delay2 = 1000 / T;
    static int time_count2 = time_delay2;

    Navigator::NavigationMode navimode = maindeal->navigator()->getNavigationMode();
    if ( navimode != Navigator::NAVIGATION_SLAM ) { return false; }

    int    startId = maindeal->navigator()->stations_list().front().id;
    int    endId   = maindeal->navigator()->stations_list().back().id;
    double Erad, Eh, Edis;
    maindeal->navigator()->getSlamParam( Erad, Eh, Edis );
    //    Point  start            = *maindeal->navigator()->startStation().point();
    //    Point  target           = *maindeal->navigator()->targetStation().point();
    double start2target_dis = maindeal->navigator()->getSlamRouteLength();  // Utility::calcDistance( start, target );
    // UPDATE 2022.08.10 停障和直接绕行判断
    QString tips = "警告，前方有障碍,停障等待障碍物远离！";
    //（1）查表获取信息是否是停障路段或者直接绕行路段
    bool stop    = maindeal->navigator()->slamRoutePlanning()->isNeedStopBarrier( 1, startId, endId, start2target_dis, Edis );
    bool running = maindeal->navigator()->slamRoutePlanning()->isNeedStopBarrier( 0, startId, endId, start2target_dis, Edis );
    if ( stop ) {
        time_count = time_delay;
        tips       = "警告，进入停障路段，并且前方有障碍";
    }
    else if ( running ) {
        time_count = 0;
    }
    //（2）等待时间绕障 根据绕障程度控制停障碍，大于设定参数就停障，等障碍物远离
    if ( fabs( Eh ) > Param::max_point2line_to_around ) {
        time_count = time_delay;
        tips       = "警告，绕障碍距离过大，停障等待障碍物远离！";
    }
    Navigator::NaviStep       slamNavistep   = maindeal->navigator()->getSlamNaviStep();
    Navigator::SlamNaviStatus slamNaviStatus = maindeal->navigator()->getSlamNaviStatus();
    // UPDATE 2022.08.12 ROS其余异常状态触发时间超过1秒，就报停障停车。
    if ( ( slamNavistep == Navigator::TRACING || slamNavistep == Navigator::READY )  // UPDATE 2022.08.19 终点被占据TRACING和READY都要触发停障
         && slamNaviStatus != Navigator::NOMAL && slamNaviStatus != Navigator::AROUND_THE_OBS ) {
        if ( --time_count2 <= 0 ) return true;
    }
    else {
        time_count2 = time_delay2;
    }
    // UPDATE 2022.07.15 正常直线行驶，贴近目标路线，才复位延时时间。避免绕行过程的状态突变导致的停顿
    if ( slamNavistep == Navigator::TRACING && slamNaviStatus == Navigator::NOMAL && fabs( Eh ) < 100 ) { time_count = time_delay; }
    if ( slamNavistep == Navigator::TRACING && slamNaviStatus == Navigator::AROUND_THE_OBS ) {
        // UPDATE 2022.08.18 先给车旋转完，再触发绕障停车
        if ( fabs( Eh ) < 300 && fabs( Erad ) > M_PI_4 ) { return false; }
        if ( --time_count >= 0 )  //知道要绕障碍，维持停障足够时间之后再开始绕行，如果停障路段则不恢复绕行
        {
            sigSendTips( tips );
            return true;
        }
    }

    return false;
}

/**
 * @brief Agv::isNeedEmergencyStop 判断agv是否需要急停.
 * @param isForward 前进的方向 暂无使用
 * @return true:需要急停 false:不需要
 */

bool Agvhardware::isNeedEmergencyStop() {
    if ( io_module->getDi( IO::EMERGENCY_STOP ) == IO::RESET ) {
        emit sigSendTips( "错误，急停按钮被按下,所有电机驱动器失能!" );
        return true;
    }

#if FOKRLIFT_IO == FOKRLIFT_IO_OLD
    // update 20230708
    if ( io_module->getDi( IO::BUMPER_STRIP ) == IO::SET ) {
        emit sigSendTips( "错误，防撞条触发,所有电机驱动器失能!" );
        return true;
    }
#elif FOKRLIFT_IO == FOKRLIFT_IO_NEW
    if ( io_module->getDi( IO::BUMPER_STRIP ) == IO::RESET ) {
        emit sigSendTips( "错误，防撞条触发,所有电机驱动器失能!" );
        //     canModule()->CanSetSpeedAngle(0, 0);
        return true;
    }
#endif

    return false;
}

/**
 * @brief Agv::isHardwareError 读取输入口，判断agv是否存在硬件错误。其同时会发出信号
 * @return true有错误，false没错误
 */
bool Agvhardware::isHardwareError() {
    bool       is_alm     = false;
    int        time_limit = 10;
    static int time       = 0;
    QString    tips;
    //    if ( io_module->getDi( IO::LEFT_SERVO_DRIVER_ALARM ) == IO::SET ) {
    //        tips   = "错误，左伺服报警!";
    //        is_alm = true;
    //    }

    //信号消抖处理，避免误判
    if ( is_alm ) {
        if ( time++ >= time_limit ) {
            // UPDATE 2022.07.11伺服报警后获取驱动的错误信息
            if ( tips == "错误，左伺服报警!" || tips == "错误，右伺服报警!" ) { forklift_drive_module->GetDriveErrInfo(); }
            emit sigSendTips( tips );
            return true;
        }
    }
    else {
        time = 0;
    }
    return false;
}

Agvhardware::AgvState Agvhardware::getAgvState() const {
    return agv_state;
}

ForkliftDrive* Agvhardware::canModule() const {
    return forklift_drive_module;
}

IO* Agvhardware::ioModule() const {
    return io_module;
}

// Qrcode* Agvhardware::qrcodeModule() const { return qrcode_module; }

ChargingModel* Agvhardware::chargingPileModule() const {
    return charging_module;
}

Battery_Ropeencoder_can* Agvhardware::Battery_Ropeencoder_canModule() const {
    return Battery_Ropeencoder_can_Module;
}

// Battery_YJT* Agvhardware::batteryModule() const { return battery_module; }

void Agvhardware::setL_R_obs_open( bool l, bool r ) {
    left_obs_flag  = l;
    right_obs_flag = r;
}

bool Agvhardware::get_l_obs_open() {
    return left_obs_flag;
}

bool Agvhardware::get_r_obs_open() {
    return right_obs_flag;
}

/*****************************************************************
* 函数名称： setForkliftState
* 功能描述： 设置叉车货叉运动状态
*           state: STOP HIGHEST HIGH LOW LOWEST
* 参数说明： state:返回货叉状态
*
* 返回值：   void
******************************************************************/

//设定货叉动作状态
void Agvhardware::setForkliftState( const Agvhardware::ForkliftState& state ) {
    // 急停状态下不处理
    if ( maindeal->navigator()->isEmergencyStop() ) {
        fork_lift_action = STOP;
        canModule()->push_fork_stop();
        return;
    }
    fork_lift_action = state;
}

void Agvhardware::ForkliftHandle() {
    switch ( fork_lift_action ) {
    case STOP:                          //停止
        canModule()->push_fork_stop();  // 立即执行 停止动作
        if ( getForkPosState() == Agvhardware::FORK_RAISING || getForkPosState() == Agvhardware::FORK_FALLING ) {
            emit sigSendTips( " 货叉停止升降 " );
            setAgvState( Agvhardware::AGV_LIFT_STOP );
        }
        break;

    case RAISE:  //上升中

        if ( getAgvState() == AGV_ARRIVE ) { setAgvState( AGV_FORK ); }

        if ( getForkPosState() != FORK_TOP ) {
            canModule()->push_fork_upper();
            emit sigSendTips( " 货叉开始下降 " );
        }
        else if ( getForkPosState() == FORK_TOP ) {
            canModule()->push_fork_stop();
            fork_lift_action = STOP;
            emit sigSendTips( " 货叉上升完成 " );
        }
        // 完成动作 切回到达状态
        if ( getAgvState() == AGV_FORK ) { setAgvState( AGV_ARRIVE ); }
        break;
    case FALL:  //下降中

        if ( getAgvState() == AGV_ARRIVE ) { setAgvState( AGV_FORK ); }

        if ( getForkPosState() != FORK_BOTTOM ) {
            canModule()->push_fork_down();
            emit sigSendTips( " 货叉开始下降 " );
        }
        else if ( getForkPosState() == FORK_BOTTOM ) {
            canModule()->push_fork_stop();
            fork_lift_action = STOP;
            emit sigSendTips( " 货叉下降完成 " );
        }
        // 完成动作 切回到达状态
        if ( getAgvState() == AGV_FORK ) { setAgvState( AGV_ARRIVE ); }
        // 货叉放下就算空载
        //        goods_state = EMPTY;
        break;
    }
    // 判断货物状态
    if ( io_module->getDi( IO::LEFT_GOODS_DETECTION ) == IO::SET && io_module->getDi( IO::RIGHT_GOODS_DETECTION ) == IO::SET ) { goods_state = PALLET_AND_GOODS; }
    else if ( io_module->getDi( IO::LEFT_PALLET_DETECTION ) == IO::SET && io_module->getDi( IO::RIGHT_PALLET_DETECTION ) == IO::SET ) {
        goods_state = PALLET;
    }
    else {
        goods_state = EMPTY;
    }
}

//货叉运动状态 从下到上依次为 低低位 低位 高位 高高位。处于上下限位时货叉停止运动
Agvhardware::ForkPosState Agvhardware::getForkPosState() {
    if ( io_module->getDi( IO::LIFIT_UPPER_LIMIT ) == IO::SET && io_module->getDi( IO::LIFIT_DOWN_LIMIT ) == IO::RESET && io_module->getDi( IO::LIFIT_MIDDLE_LIMIT ) == IO::RESET ) {
        return Agvhardware::FORK_TOP;
    }
    else if ( io_module->getDi( IO::LIFIT_UPPER_LIMIT ) == IO::RESET && io_module->getDi( IO::LIFIT_DOWN_LIMIT ) == IO::SET && io_module->getDi( IO::LIFIT_MIDDLE_LIMIT ) == IO::RESET ) {
        return Agvhardware::FORK_BOTTOM;
    }
    else if ( io_module->getDi( IO::LIFIT_UPPER_LIMIT ) == IO::RESET && io_module->getDi( IO::LIFIT_DOWN_LIMIT ) == IO::RESET && io_module->getDi( IO::LIFIT_MIDDLE_LIMIT ) == IO::SET ) {
        return Agvhardware::FORK_ORIGIN;
    }
    else {
        switch ( fork_lift_action ) {
        case RAISE:
            return Agvhardware::FORK_RAISING;
        case FALL:
            return Agvhardware::FORK_FALLING;
        case STOP:
        default:
            return Agvhardware::FORK_UNKNOWN;
        }
    }
}

void Agvhardware::setAlarm( bool isAlarm ) {
    is_alarm_ = isAlarm;
}

void Agvhardware::resetHardwareState() {
    //打开伺服电机的使能
    //    io_module->setDo( IO::LEFT_SERVO_DRIVER_ENABLE, IO::SET );
    //    io_module->setDo( IO::RIGHT_SERVO_DRIVER_ENABLE, IO::SET );
    //    transmitDriverDisable();               //
    //    transmit_order       = STOP_TRANSMIT;  //复位
    setAgvState( AGV_STOP );
    //    is_cannot_transmit   = false;
    //    is_transmit_overtime = false;
    //    is_transmit_enable   = false;
}

void Agvhardware::ChangeObs( int left, int right, int back, int slam_laser ) {
    // UPDATE 2023.06.13 加入避障区域4
    //左
    if ( left == 1 ) {  // min           兴颂传感器61区域    雷眸3区域
        io_module->setDo( IO::LEFT_OBSTACLE_INPUT_1, IO::RESET );
        io_module->setDo( IO::LEFT_OBSTACLE_INPUT_2, IO::SET );
    }
    else if ( left == 2 ) {  // mid      兴颂传感器62区域    雷眸2区域
        io_module->setDo( IO::LEFT_OBSTACLE_INPUT_1, IO::SET );
        io_module->setDo( IO::LEFT_OBSTACLE_INPUT_2, IO::RESET );
    }
    else if ( left == 3 ) {  // max      兴颂传感器63区域    雷眸1区域
        io_module->setDo( IO::LEFT_OBSTACLE_INPUT_1, IO::RESET );
        io_module->setDo( IO::LEFT_OBSTACLE_INPUT_2, IO::RESET );
    }
    else if ( left == 4 ) {  // max      兴颂传感器60区域    雷眸4区域
        io_module->setDo( IO::LEFT_OBSTACLE_INPUT_1, IO::SET );
        io_module->setDo( IO::LEFT_OBSTACLE_INPUT_2, IO::SET );
    }

    //右
    if ( right == 1 ) {  // min
        io_module->setDo( IO::RIGHT_OBSTACLE_INPUT_1, IO::RESET );
        io_module->setDo( IO::RIGHT_OBSTACLE_INPUT_2, IO::SET );
    }
    else if ( right == 2 ) {  // mid
        io_module->setDo( IO::RIGHT_OBSTACLE_INPUT_1, IO::SET );
        io_module->setDo( IO::RIGHT_OBSTACLE_INPUT_2, IO::RESET );
    }
    else if ( right == 3 ) {  // max
        io_module->setDo( IO::RIGHT_OBSTACLE_INPUT_1, IO::RESET );
        io_module->setDo( IO::RIGHT_OBSTACLE_INPUT_2, IO::RESET );
    }
    else if ( right == 4 ) {  // max
        io_module->setDo( IO::RIGHT_OBSTACLE_INPUT_1, IO::SET );
        io_module->setDo( IO::RIGHT_OBSTACLE_INPUT_2, IO::SET );
    }

    //后
    if ( back == 1 ) {  // min
        io_module->setDo( IO::BACK_OBSTACLE_INPUT_1, IO::RESET );
        io_module->setDo( IO::BACK_OBSTACLE_INPUT_2, IO::SET );
    }
    else if ( back == 2 ) {  // mid
        io_module->setDo( IO::BACK_OBSTACLE_INPUT_1, IO::SET );
        io_module->setDo( IO::BACK_OBSTACLE_INPUT_2, IO::RESET );
    }
    else if ( back == 3 ) {  // max
        io_module->setDo( IO::BACK_OBSTACLE_INPUT_1, IO::RESET );
        io_module->setDo( IO::BACK_OBSTACLE_INPUT_2, IO::RESET );
    }
    else if ( back == 4 ) {  // max
        io_module->setDo( IO::BACK_OBSTACLE_INPUT_1, IO::SET );
        io_module->setDo( IO::BACK_OBSTACLE_INPUT_2, IO::SET );
    }

    //导航雷达
    if ( slam_laser == 1 ) {  // min
        io_module->setDo( IO::NAV_LASER_INPUT_1, IO::RESET );
        io_module->setDo( IO::NAV_LASER_INPUT_2, IO::SET );
    }
    else if ( slam_laser == 2 ) {  // mid
        io_module->setDo( IO::NAV_LASER_INPUT_1, IO::SET );
        io_module->setDo( IO::NAV_LASER_INPUT_2, IO::RESET );
    }
    else if ( slam_laser == 3 ) {  // max
        io_module->setDo( IO::NAV_LASER_INPUT_1, IO::RESET );
        io_module->setDo( IO::NAV_LASER_INPUT_2, IO::RESET );
    }
    else if ( slam_laser == 4 ) {  // max
        io_module->setDo( IO::NAV_LASER_INPUT_1, IO::SET );
        io_module->setDo( IO::NAV_LASER_INPUT_2, IO::SET );
    }
}

bool Agvhardware::isAlarm() const {
    return is_alarm_;
}
